package def;

import SimEnvironment.*;

// BeamAndBallRegul class to be written by you
public class BeamAndBallRegul extends Thread {
	
  // Declarations
	private AnalogSource analogInAngle;
	private AnalogSource analogInPosition;
	private AnalogSink analogOut;
	private AnalogSink analogRef;
	
	private ReferenceGenerator refgen;
	private PI pi_ctrl;
	private PID pid_ctrl;
	
	public BeamAndBallRegul(ReferenceGenerator ref, BeamAndBall beam, int pri) {
		refgen = ref;
		pi_ctrl = new PI("PI");
		pid_ctrl = new PID("PID");
		
		analogInPosition = beam.getSource(0);
		analogInAngle = beam.getSource(1);
		analogOut = beam.getSink(0);
		analogRef = beam.getSink(1);
		
		setPriority(pri);
	}

	private double outer_max=10, outer_min=-10;
	private double limit_outer(double u) {
		if (u < outer_min) {
			u = outer_min;
		} else if (u > outer_max) {
			u = outer_max;
		} 
		return u;
	}
	
	private double inner_max=10, inner_min=-10;
	private double limit_inner(double u) {
		if (u < inner_min) {
			u = inner_min;
		} else if (u > inner_max) {
			u = inner_max;
		} 
		return u;
	}
	
	
	public void run() {
		long t = System.currentTimeMillis();
		while (true) {
			// Read inputs
			
			double pos = analogInPosition.get();
			double angle = analogInAngle.get();
			double ref = refgen.getRef();
			analogRef.set(ref); // Only for the plotter animation
			
			double angle_ref;
			/* outer loop */
			synchronized (pid_ctrl) { // To avoid parameter changes in between
				// Compute control signal
				angle_ref = limit_outer(pid_ctrl.calculateOutput(pos, ref));
				
				// Update state
				pid_ctrl.updateState(angle_ref);
			}
			
			/* inner loop */
			synchronized (pi_ctrl) { // To avoid parameter changes in between
				// Compute control signal
				double u = limit_inner(pi_ctrl.calculateOutput(angle, angle_ref));
				
				analogOut.set(u);
				
				// Update state
				pi_ctrl.updateState(u);
			}
			
			t = t + pi_ctrl.getHMillis();
			long duration = t - System.currentTimeMillis();
			if (duration > 0) {
				try {
					sleep(duration);
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
			}
		}
	}
}

